/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/opSrc/operation_charge.cpp
 * @Description  : 实现了OperationCharge类，用于管理充电操作。
 *                 主要功能包括初始化充电操作、准备充电、开始充电、执行充电和结束充电。
 *                 使用了服务客户端与AGV的充电系统进行交互，通过IO输出控制充电状态。
 *                 代码中包含了线程安全的锁机制，以确保充电操作的同步。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:30:16
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "operation_charge.h"
#include <vector>
#include "public_kernel.h" 

/**
 * @brief 构造函数
 *
 * 初始化OperationCharge对象，设置服务客户端，并初始化充电IO索引
 *
 * @param action agv_msgs::msg::Action类型的参数，表示动作信息
 */
OperationCharge::OperationCharge(const agv_msgs::msg::Action& action)
    : Operation(action), charge_io_index_(-1)
{
}

/**
 * @brief 初始化
 *
 * 根据是否定义了AGV_DEBUG_NAV宏以及client_对象是否有效来初始化OperationCharge对象。
 *
 * @return 如果初始化成功返回true，否则返回false。
 */
bool OperationCharge::Init()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    client_ = ROSNode()->create_client<agv_srvs::srv::BatteryCharge>("/do_charge");

    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        DEBUG_ERROR_OUT("Interrupted while waiting for the service. Exiting.");
        return false;
      }
      DEBUG_OUT("do_charge service not available, waiting again...");
    }
    return true;
}

/**
 * @brief 准备开始充电操作
 *
 * 如果定义了AGV_DEBUG_NAV宏，则直接返回true。否则，会检查循环标记的有效性，如果无效则返回false并打印错误信息，否则返回true。
 *
 * @return 如果成功准备开始充电操作，则返回true；否则返回false。
 */
bool OperationCharge::ReadyStart()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif

    if (!TryCycleMarkCheck()) {
        DEBUG_ERROR_OUT("OperationCharge: qrId is not Valid");
        return false;
    }

    return true;
}

/**
 * @brief 开始充电操作
 *
 * 该函数用于开始充电操作。
 *
 * @return 如果充电操作成功启动，则返回 true；否则返回 false。
 */
bool OperationCharge::StartAction()
{
    std::lock_guard<std::mutex> locker(mutex_);
    finished_ = false;
    SetStopCharging(false);

#ifdef AGV_DEBUG_NAV
    return true;
#endif

    bool ret = SetChargeIoOut(true);
    DEBUG_OUT("OperationCharge start :ret:" << ret);
    return ret;
}

/**
 * @brief 执行充电操作
 *
 * 该函数用于执行充电操作，并根据条件判断是否完成充电。
 *
 * @return 如果充电操作完成，则返回true；否则返回false。
 */
bool OperationCharge::Work()
{
    std::lock_guard<std::mutex> locker(mutex_);

#ifdef AGV_DEBUG_NAV
    return total_count_++ > 100; // 5s-500
#endif

    bool bCloseChrageFinish = false;
    if (GetStopCharging()) {
        bCloseChrageFinish = SetChargeIoOut(false);
        DEBUG_OUT("GetStopCharging bCloseChrageFinish:" << bCloseChrageFinish);
    }

    return bCloseChrageFinish;
}

/**
 * @brief 结束充电操作
 *
 * 该函数用于结束充电操作。在调试模式下，直接返回true。
 * 在非调试模式下，调用SetChargeIoOut函数将充电IO输出设置为false，
 * 并输出调试信息，然后返回true。
 *
 * @return 总是返回true
 */
bool OperationCharge::EndAction()
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    bool ret = SetChargeIoOut(false);
    DEBUG_OUT("OperationCharge EndAction :ret " << ret);
    return true;
}

/**
 * @brief 设置充电IO输出状态
 *
 * 设置充电IO的输出状态，通过调用服务实现。
 *
 * @param open IO输出状态，true表示打开，false表示关闭
 * @return 设置成功返回true，否则返回false
 */
bool OperationCharge::SetChargeIoOut(bool open)
{
    // IO列表

    // 设置充电IO

    // 调用服务
    auto request = std::make_shared<agv_srvs::srv::BatteryCharge::Request>();
    request->start = open;

    using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::BatteryCharge>::SharedFuture;
    auto response_received_callback = [open](ServiceResponseFuture future) {
        if (future.get()->success)
            DEBUG_OUT("charge operation success bOpen:" << open);
        else
            DEBUG_ERROR_OUT("charge operation fail bOpen:" << open);
    };

    auto result = client_->async_send_request(request, response_received_callback);
  
    if (result.valid()) {
        DEBUG_OUT("Request successfully sent");
        return true;
    } else {
        DEBUG_OUT("Failed to send request");
        return false;
    }
}